#include "bmi160.h"
#include "fusion.h"
#include "ZYM568.h"
#include "component.h"
#include "device.h"
#include "i2c_sw.h"
#include <math.h>

/* 调试打印接口 */
#define PSENSOR_LOG(format, ...) OSAL_LOG(C_CYAN format C_NONE, ##__VA_ARGS__)
#define __PSENSOR_LOG(format, ...) //__OSAL_LOG(C_CYAN format C_NONE, ##__VA_ARGS__)

/* 读imu事件 */
#define EVENT_IMU_PROCESS (0X00000001)

// #define USE_CUSTOM_ALGORITHM //不使用模块提供的磁力库算法
// #define PSENSOR_DEBUG

#define DRIVELOWPOWERTIM 1000 //设备休眠唤醒间隔时间
#define DOOROMAGCLASS_1 4.0f  //全功率模式下，环境噪声等级(3~15)
#define DOOROMAGCLASS_2 8.0f  //低功耗模式下，环境噪声等级(3~15)

#if defined(USE_CUSTOM_ALGORITHM)
#define DOOROPENANGLE 20.0f //低功耗模式开关门角度
#define DEFAULT_Q 0.01		//卡尔曼滤波过程噪声
#define DEFAULT_R 0.5		//卡尔曼滤波测量噪声
#else
#define DOOROPENANGLE 3.0f //全功率模式开关门角度
#endif

#define PSENSOR_STABLE_TIME 500		//正常模式传感器稳定时间(ms)
#define CALIBRATION_TIMEOUT 1000	//校准超时时间(ms)

typedef enum
{
	PSENSOR_MODE_IDLE,
	PSENSOR_MODE_CALIBRATION,
	PSENSOR_MODE_NORMOL,
	PSENSOR_MODE_LOWPOWER,
} Psensor_Mode_enum_t;

typedef enum
{
	DOOR_STATUS_OPENED = 1,
	DOOR_STATUS_CLOSED,
	DOOR_STATUS_UNKNOWN,
} Door_Status_enum_t;

//系统变量
__EFRAM static Psensor_Mode_enum_t runMode = PSENSOR_MODE_IDLE;
__EFRAM static Door_Status_enum_t door_status = DOOR_STATUS_UNKNOWN;
__EFRAM static short runPeriod = 10;
#if !defined(USE_CUSTOM_ALGORITHM)
// __EFRAM static uint8_t ExternRam[38] = {0};
#endif
static PsensorMsg_t psensor_msg;
static uint32_t calibration_start_time = 0;

static void delay_us(uint32_t nTime)
{
	if (nTime > 1000)
	{
		Device_DelayMs(nTime / 1000);
	}
	else
	{
		Device_DelayUs(nTime);
	}
}

static void delay_ms(uint32_t nTime)
{
	Device_DelayMs(nTime);
}

/*
	模组芯片初始化
	初始化成功返回1
*/
static int Psensor_Init(void)
{
	if (bmi160_init() == 0) //初始化失败
	{
		PSENSOR_LOG("bmi160 init err\r\n");
		OSAL_MessagePublishErrorCode(ERRCODE_TYPE_PSENSOR, 1);
		return 0;
	}
	if (ZYM568_init() == 0) //初始化失败
	{
		OSAL_MessagePublishErrorCode(ERRCODE_TYPE_PSENSOR, 1);
		PSENSOR_LOG("zym568 init err\r\n");
		return 0;
	}
	OSAL_MessagePublishErrorCode(ERRCODE_TYPE_PSENSOR, 0);

	i2c_init();

	IL005_delay_us = delay_us;
	IL005_delay_ms = delay_ms;
	IL005_i2c_start = i2c_start;
	IL005_i2c_stop = i2c_stop;
	IL005_i2c_ack = i2c_ack;
	IL005_i2c_noack = i2c_noack;
	IL005_i2c_waitack = i2c_waitack;
	IL005_i2c_wakeup = i2c_wakeup;
	IL005_i2c_sendbyte = i2c_sendbyte;
	IL005_i2c_receivebyte = i2c_receivebyte;
	IL005_i2c_init = i2c_init; //加密芯片IIC驱动
	Drive_i2c_init = i2c_init; //传感器IIC驱动

	return 1;
}

/*
	传感器进入低功耗
*/
static void SenserLowpower(void)
{
	bmi160_lowpowermode();
	ZYM568_lowpowermode();
}

#if defined(USE_CUSTOM_ALGORITHM)
__EFRAM static float origin_closedoor[3] = {0};
static uint16_t cal_cnt = 0;
static double cal_data_and[3] = {0};
static void calibration_init(void)
{
	cal_cnt = 0;
	memset(cal_data_and, 0x00, sizeof(cal_data_and));
}

static void calibration_process(void)
{
	origin_closedoor[0] = cal_data_and[0] / cal_cnt;
	origin_closedoor[1] = cal_data_and[1] / cal_cnt;
	origin_closedoor[2] = cal_data_and[2] / cal_cnt;
	PSENSOR_LOG("cal_cnt : %d, origin_closedoor = %.0f, %.0f, %.0f\r\n", cal_cnt, origin_closedoor[0], origin_closedoor[1], origin_closedoor[2]);
}

/*-------------------------------------------------------------------------------------------------------------*/
/*	
	Q:过程噪声，Q增大，动态响应变快，收敛稳定性变坏
	R:测量噪声，R增大，动态响应变慢，收敛稳定性变好	
*/
/*-------------------------------------------------------------------------------------------------------------*/

static double KalmanFilter(const double ResrcData, double ProcessNiose_Q, double MeasureNoise_R, double InitialPrediction)
{
	static uint8_t init_flag = 0;
	static double x_last = 0;
	static double p_last = 0;

	double x_mid = x_last;
	double x_now;
	double p_mid;
	double p_now;
	double kg;

	if (init_flag == 0)
	{
		init_flag = 1;
		x_last = ResrcData; //更新系统状态值
	}

	x_mid = x_last;							  //x_last=x(k-1|k-1),x_mid=x(k|k-1)
	p_mid = p_last + ProcessNiose_Q;		  //p_mid=p(k|k-1),p_last=p(k-1|k-1),Q=噪声
	kg = p_mid / (p_mid + MeasureNoise_R);	  //kg为kalman filter，R为噪声
	x_now = x_mid + kg * (ResrcData - x_mid); //估计出的最优值

	p_now = (1 - kg) * p_mid; //最优值对应的covariance

	p_last = p_now; //更新covariance值
	x_last = x_now; //更新系统状态值

	return x_now;
}

/*
//计算磁场角
*/
static float _get_angle(float x, float y, float z)
{
	return sqrt(x * x + y * y + z * z);
}

static short LowpowerGetDoorPose(short deltTime, float AngleValve, float magValve, float *angles, short mode)
{
	short sensordata[3] = {0}; //采样的传感器数据
	float rawdata, outdata;
	uint8_t magenble = ZYM568_Get_Data(&sensordata[0], deltTime);
	if (magenble == 1) //数据更新成功
	{
		/* 计算空间两点变化 */
		sensordata[0] -= origin_closedoor[0];
		sensordata[1] -= origin_closedoor[1];
		sensordata[2] -= origin_closedoor[2];
		rawdata = (float)_get_angle(sensordata[0], sensordata[1], sensordata[2]);
		if(mode != 1) //非功耗下滤波
		{
			outdata = (float)KalmanFilter(rawdata, DEFAULT_Q, DEFAULT_R, 0); //卡尔曼滤波
		}
		printf("%.1f\t%.1f\r\n", rawdata, outdata);
		outdata = rawdata / 2;
		*angles = outdata;

#if defined(PSENSOR_DEBUG)
		__PSENSOR_LOG("\r\n");
		for (int i = 0; i < 3; i++)
		{
			__PSENSOR_LOG("%d ", sensordata[i]);
		}
		__PSENSOR_LOG("rawdata = %.1f, outdata = %.1f\r\n", rawdata, outdata);
#endif

		if (outdata < AngleValve)
		{
			return 0;
		}
		else
		{
			return 1;
		}
	}
	return 2; //数据未更新
}

#else

static short LowpowerGetDoorPose(
	short deltTime,
	float AngleValve,
	float magValve,
	float *angles,
	short mode) //低功耗获取门的姿态
{
	short sensordata[9] = {0}; //采样的传感器数据
	short status = 0xff;
	uint8_t magenble = ZYM568_Get_Data(&sensordata[6], deltTime); //读传感器数据，计算门状态

#if defined(PSENSOR_DEBUG)
	__PSENSOR_LOG("\r\n");
	for (int i = 6; i < 9; i++)
	{
		__PSENSOR_LOG("%d ", sensordata[i]);
	}
	__PSENSOR_LOG("\r\n");
#endif

	if (magenble != 0) //数据读取成功
	{
		//计算门的状态与角度
		status = fuison_func(sensordata, magenble, deltTime, angles, AngleValve, magValve, mode);
	}
	return status;
}

#endif
/*******************************************************************************************************
short sensordata[6]:GyrX,GyrY,GyrZ,AccX,AccY,AccZ
unsigned char magEnable: 当前地磁数据是否更新，如果更新则输入1，否则输入0
short TimeCount: 调用间隔时间，单位ms
float AngleValve: 使用陀螺时当前角度阀值，建议设置为15
float magValve: 环境磁场噪声参数，推荐设置
short pureMag: 当前是否为纯地磁模式，是输入1，否输入0
float *angles: 当前门的角度反馈，此参数在非纯磁模式下有效，单位为度
return：0 当前关门状态
        1 当前开门状态
		2 产生错误
********************************************************************************************************/
static short GetDoorPose(
	short deltTime,
	float AngleValve,
	float magValve,
	float *angles) //获取门的姿态
{
	short sensordata[9] = {0}; //采样的传感器数据
	//读传感器数据
	bmi160_read_gyroAcc_rate(&sensordata[3], &sensordata[0]);
	uint8_t magenble = ZYM568_Get_Data(&sensordata[6], deltTime);

	//计算门的状态与角度
	return fuison_func(sensordata, magenble, deltTime, angles, AngleValve, magValve, 0);
}

/*
传感器与门状态0度校准,校准过程中
要不断调用此函数获取传感器数据，直到校准完成。
deltTime 调用时间间隔
*/
static int SensorCalibration(short deltTime) //
{
	short sensordata[9] = {0}; //采样的传感器数据
	//读传感器数据
	bmi160_read_gyroAcc_rate(&sensordata[3], &sensordata[0]);
	uint8_t magenble = ZYM568_Get_Data(&sensordata[6], deltTime);

#if defined(PSENSOR_DEBUG)
	// __PSENSOR_LOG("\r\n");
	// for (int i = 0; i < 9; i++)
	// {
	// 	__PSENSOR_LOG("%d ", sensordata[i]);
	// }
	// __PSENSOR_LOG("\r\n");
#endif

#if defined(USE_CUSTOM_ALGORITHM)
	if (magenble == 1) //数据有效
	{
		cal_data_and[0] += sensordata[6];
		cal_data_and[1] += sensordata[7];
		cal_data_and[2] += sensordata[8];
		cal_cnt++;
	}
#endif

	//传感器校准，并设置使用的方向
	short ret = fusion_init(sensordata, magenble, 0);
	if (ret == 0)
	{
		psensor_msg.msg = PSENSOR_CAL_FINISHED;
		OSAL_MessagePublish(&psensor_msg, sizeof(PsensorMsg_t));
		PSENSOR_LOG("Calibration is complete\r\n");
		return 1; //校准完成
	}
	else if (ret & 0x200)
	{
		PSENSOR_LOG("psensor encryption chip driver err : %#x\r\n", ret);
	}
	else if (ret & 0x100)
	{
		PSENSOR_LOG("psensor encryption chip key err : %#x\r\n", ret);
	}
	else if((ret & 0xfff0) == 0x0000)
	{
		__PSENSOR_LOG("mag cal finished : %#x\r\n", ret);
		return 2; //地磁校准完成，陀螺仪校准未完成
	}
	return 0; //校准未完成
}

static int32_t Psensor_Imu_Process(void)
{
	int32_t ret = 0;
	static short mode = 0;
	static short last_mode = 0xff;
	static int16_t stable_time = 0;
	float doorAngles[3] = {0.0f}; //门打开的角度，单位为度
	static uint32_t last_time = 0;

	/* 校准状态，注意此时门要在关闭且静止 */
	if (runMode == PSENSOR_MODE_CALIBRATION)
	{
		runPeriod = OSAL_GetTickCount() - last_time;
		last_time = OSAL_GetTickCount();
		int cal_status = SensorCalibration(runPeriod);
		if (cal_status == 1) //陀螺仪校准完成
		{
			PSENSOR_LOG("Sensor calibration done\r\n");
			OSAL_SetTaskStatus(TASK_STA_NORMAL); //允许系统休眠
		#if 1 //全功率模式
			runMode = PSENSOR_MODE_NORMOL;
			runPeriod = 10;
			OSAL_EventRepeatCreate(COMP_PSENSOR, EVENT_IMU_PROCESS, runPeriod, EVT_PRIORITY_MEDIUM);
		#else //纯地磁模式
			runMode = PSENSOR_MODE_LOWPOWER;
			runPeriod = 30;
			OSAL_EventRepeatCreate(COMP_PSENSOR, EVENT_IMU_PROCESS, runPeriod, EVT_PRIORITY_MEDIUM);
		#endif
		}
		else
		{
			uint32_t cur_time = OSAL_GetTickCount();
			if(cur_time - calibration_start_time > CALIBRATION_TIMEOUT) //校准超时
			{
				PSENSOR_LOG("Sensor calibration timeout, %d\r\n", cal_status);
				OSAL_SetTaskStatus(TASK_STA_NORMAL); //允许系统休眠
				if(cal_status == 2)
				{
					runMode = PSENSOR_MODE_LOWPOWER;
					runPeriod = 30;
					OSAL_EventRepeatCreate(COMP_PSENSOR, EVENT_IMU_PROCESS, runPeriod, EVT_PRIORITY_MEDIUM);
				}
				else
				{
					runMode = PSENSOR_MODE_IDLE;
					OSAL_EventDelete(COMP_PSENSOR, EVENT_IMU_PROCESS);
				}
				psensor_msg.msg = PSENSOR_CAL_FAILED;
				OSAL_MessagePublish(&psensor_msg, sizeof(PsensorMsg_t));
			}
		}
	#if defined(USE_CUSTOM_ALGORITHM)
		if(runMode == PSENSOR_MODE_LOWPOWER || runMode == PSENSOR_MODE_NORMOL)
		{
			calibration_process();
		}
	#endif
		
	}

	/* 门状态计算 */
	else if (runMode == PSENSOR_MODE_NORMOL || PSENSOR_MODE_LOWPOWER)
	{
		if (runMode == PSENSOR_MODE_NORMOL)
		{
			mode = GetDoorPose(runPeriod, DOOROPENANGLE, DOOROMAGCLASS_1, doorAngles); //获取门的姿态
		}
		else
		{
			mode = LowpowerGetDoorPose(runPeriod, DOOROPENANGLE, DOOROMAGCLASS_2, doorAngles, 2);
		}
		__PSENSOR_LOG("runMode : %d, door : %d, doorAngles = %.1f, %.1f, %.1f\r\n", runMode, mode, doorAngles[0], doorAngles[1], doorAngles[2]);

		if (mode == 0 || mode == 1)
		{
			if (mode != last_mode)
			{
				stable_time = 0;
			}
			last_mode = mode;
		}

		if (mode == 0)
		{
			if (door_status != DOOR_STATUS_CLOSED)
			{
				stable_time += runPeriod;
				if (stable_time >= PSENSOR_STABLE_TIME)
				{
					runMode = PSENSOR_MODE_IDLE; //关门后关闭传感器
					OSAL_EventDelete(COMP_PSENSOR, EVENT_IMU_PROCESS);
					door_status = DOOR_STATUS_CLOSED;
					ret = DOOR_STATUS_CLOSED;
					psensor_msg.msg = PSENSOR_CLOSED;
					OSAL_MessagePublish(&psensor_msg, sizeof(PsensorMsg_t));
					PSENSOR_LOG("close door, runMode : %d, doorAngles = %.1f, %.1f, %.1f\r\n", runMode, doorAngles[0], doorAngles[1], doorAngles[2]);
				}
				__PSENSOR_LOG("psensor stable time : %d ms\r\n", stable_time);
			}
		}
		else if (mode == 1)
		{
			if (door_status != DOOR_STATUS_OPENED)
			{
				stable_time -= runPeriod;
				if (stable_time <= -PSENSOR_STABLE_TIME)
				{
					door_status = DOOR_STATUS_OPENED;
					ret = DOOR_STATUS_OPENED;
					psensor_msg.msg = PSENSOR_OPENED;
					OSAL_MessagePublish(&psensor_msg, sizeof(PsensorMsg_t));
					PSENSOR_LOG("open door, runMode : %d, doorAngles = %.1f, %.1f, %.1f\r\n", runMode, doorAngles[0], doorAngles[1], doorAngles[2]);
				}
				__PSENSOR_LOG("psensor stable time : %d ms\r\n", stable_time);
			}
		}
	}
	return ret;
}

/**
  * @brief 传感器校准
  *
  * @note 应用层在开门前且上锁状态下调用该接口，并等待校准完成后再执行开锁操作
  *
  * @return 返回是否允许休眠
  */
static ErrorStatus Psensor_Calibration(void)
{
	int ret = Psensor_Init();
	if(ret)
	{
		door_status = DOOR_STATUS_CLOSED;
		calibration_start_time = OSAL_GetTickCount();
		resetFusion(); //算法初始化
	#if defined(USE_CUSTOM_ALGORITHM)
		calibration_init();
	#endif
		PSENSOR_LOG("resetFusion done\r\n");

		runMode = PSENSOR_MODE_CALIBRATION;
		runPeriod = 2;
		OSAL_EventRepeatCreate(COMP_PSENSOR, EVENT_IMU_PROCESS, runPeriod, EVT_PRIORITY_MEDIUM);
		OSAL_SetTaskStatus(TASK_STA_ACTIVE); //禁止系统睡眠
		return SUCCESS;
	}
	else //硬件错误
	{
		runMode = PSENSOR_MODE_IDLE;
		OSAL_EventDelete(COMP_PSENSOR, EVENT_IMU_PROCESS);
		psensor_msg.msg = PSENSOR_CAL_FAILED;
		OSAL_MessagePublish(&psensor_msg, sizeof(PsensorMsg_t));
	}
	return ERROR;
}

#if 0
/**
  * @brief 位置传感器定时唤醒回调
  *
  * @note 
  *
  * @return 返回是否允许休眠
  */
static int32_t Psensor_TimedWakeHandle(uint32_t dev)
{
	short mode = 0;
	float doorAngles = 0.0f; //门打开的角度，单位为度
	int32_t ret = 0;
	__NVRAM static uint8_t valid_cnt = 0;
	if(runMode == PSENSOR_MODE_LOWPOWER)
	{
#if defined(USE_CUSTOM_ALGORITHM)
	mode = LowpowerGetDoorPose(runPeriod, DOOROPENANGLE, DOOROMAGCLASS_2, &doorAngles, 1);
#else
		fuison_mag_door_WriteStaticData(ExternRam);
		mode = LowpowerGetDoorPose(runPeriod, DOOROPENANGLE, DOOROMAGCLASS_2, &doorAngles, 1);
		fuison_mag_door_ReadStaticData(ExternRam);
	#if defined(PSENSOR_DEBUG)
		__PSENSOR_LOG("ExternRam : \r\n");
		for(int i = 0; i < sizeof(ExternRam); i++)
		{
			__PSENSOR_LOG("%02x ", ExternRam[i]);
		}
		__PSENSOR_LOG("\r\n");
	#endif
#endif
		__PSENSOR_LOG("sleep mode : %d, doorAngles = %.1f\r\n", mode, doorAngles);
		if (mode == 0)
		{
			if (door_status != DOOR_STATUS_CLOSED)
			{
				ret = DOOR_STATUS_CLOSED;
				PSENSOR_LOG("sleep close door wake, runMode : %d, doorAngles = %.1f\r\n", runMode, doorAngles);
			}
		}
		else if (mode == 1)
		{
			if (door_status != DOOR_STATUS_OPENED)
			{
				ret = DOOR_STATUS_OPENED;
				PSENSOR_LOG("sleep open door wake, runMode : %d, doorAngles = %.1f\r\n", runMode, doorAngles);
			}
		}
	}
#if defined(USE_CUSTOM_ALGORITHM)
	if(ret)
	{
		if(++valid_cnt < 3)
		{
			ret = 0;
		}
	}
	else
	{
		valid_cnt = 0;
	}
#endif
	return ret;
}
COMPONENT_WAKEUP_EXPORT(COMP_PSENSOR, Psensor_TimedWakeHandle, vTIMER_1);
#endif

/** 
  * @brief  Psensor任务函数
  *
  * @note   1.任务函数内不能写阻塞代码
  *         2.任务函数每次运行只处理一个事件
  *         
  * @param  event：当前任务的所有事件
  *
  * @return 返回未处理的事件
  */
static uint32_t Psensor_Task(uint32_t event)
{
	static FlagStatus power_on = SET;

	/* 系统启动事件 */
	if (event & EVENT_SYS_START)
	{
		PSENSOR_LOG("Psensor task start\r\n");
		if (power_on == SET)
		{
			power_on = RESET;
			runMode = PSENSOR_MODE_IDLE;
		}
		else
		{
			if (runMode != PSENSOR_MODE_IDLE)
			{
				runMode = PSENSOR_MODE_LOWPOWER;
				runPeriod = 30;
				OSAL_EventRepeatCreate(COMP_PSENSOR, EVENT_IMU_PROCESS, runPeriod, EVT_PRIORITY_MEDIUM);
			}
		}
		SYS_API(Psensor_Calibration);
		return (event ^ EVENT_SYS_START);
	}

	/* 系统休眠事件 */
	if (event & EVENT_SYS_SLEEP)
	{
		SenserLowpower();
#if !defined(USE_CUSTOM_ALGORITHM)
		// fuison_mag_door_ReadStaticData(ExternRam);
#endif
		if (runMode != PSENSOR_MODE_IDLE)
		{
			OSAL_EventDelete(COMP_PSENSOR, EVENT_IMU_PROCESS);
			runMode = PSENSOR_MODE_LOWPOWER;
			runPeriod = 600;
		}
		PSENSOR_LOG("Psensor task sleep\r\n");
		return (event ^ EVENT_SYS_SLEEP);
	}

	if (event & EVENT_IMU_PROCESS)
	{
		Psensor_Imu_Process();
		return (event ^ EVENT_IMU_PROCESS);
	}

	return 0;
}
COMPONENT_TASK_EXPORT(COMP_PSENSOR, Psensor_Task, 0);
